
#include "rviz_custom_plugins/tools/map_point_tool.h"
#include <rviz_common/logging.hpp>
#include <OgreCamera.h>
#include <OgreSceneManager.h>
#include <Ogre.h>
#include <OgreRay.h>
#include <OgreViewport.h>
#include <rviz_common/display_context.hpp>
#include <rviz_common/view_manager.hpp>
#include <rviz_common/tool_manager.hpp>
#include <rviz_common/viewport_mouse_event.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <visualization_msgs/msg/interactive_marker.hpp>
#include "rviz_custom_plugins/utils/interactive_marker_manager.h"
#include <QColor>
#include <QVector3D>
#include <QApplication>
#include <omp.h>
#include <QString>
#include <cstddef>


using rviz_custom_plugins::utils::InteractiveMarkerManager;
using visualization_msgs::msg::InteractiveMarker;


namespace rviz_custom_plugins
{

namespace tools {


MapPointTool::MapPointTool()
{
    mapNotifier = BagMapNotifier::getInstance();
    connect(mapNotifier, &BagMapNotifier::pointCloudLoaded, this, &MapPointTool::handlePointCloudLoaded);

    pcdPointNormal = pcl::make_shared<pcl::PointCloud<pcl::PointXYZINormal>>();
    pcdViewPointNormal = pcl::make_shared<pcl::PointCloud<pcl::PointXYZINormal>>();

    connect(this, &MapPointTool::pointCloudFiltered, mapNotifier, &BagMapNotifier::pointCloudFiltered);
    connect(this, &MapPointTool::mapPointSelected, mapNotifier, &BagMapNotifier::mapPointSelected);
}


int MapPointTool::processMouseEvent( rviz_common::ViewportMouseEvent& event )
{
    if (event.leftUp() && (QApplication::keyboardModifiers() & Qt::AltModifier) ) {
        Ogre::Camera* camera = context_->getViewManager()->getCurrent()->getCamera();
        Ogre::Viewport* viewport = camera->getViewport();

        float screen_x = static_cast<float>(event.x) / viewport->getActualWidth();
        float screen_y = static_cast<float>(event.y) / viewport->getActualHeight();

        Ogre::Ray ray = camera->getCameraToViewportRay(screen_x, screen_y);

        Ogre::Vector3 origin = ray.getOrigin();
        Ogre::Vector3 direction = ray.getDirection();

        Eigen::Vector3f ei_origin(origin.x, origin.y, origin.z);
        Eigen::Vector3f ei_direction(direction.x, direction.y, direction.z);
        
        {
            std::ostringstream sstm;
            sstm << "Origin: " << origin.x << ", " << origin.y << ", " << origin.z;
            sstm << "Direction: " << direction.x << ", " << direction.y << ", " << direction.z;
            RVIZ_COMMON_LOG_WARNING(sstm.str().c_str());
        }

        int pclIndex = findNearestPointToCamera(ei_origin, ei_direction);

        if (pclIndex >= 0) {

            pcl::PointXYZINormal pclPoint = pcdPointNormal->points[pclIndex];
            // QString markerName = generateSnapshotPointName();

            QString nameSpace = markerThread->objectName();
            // pcl::PointXYZINormal point = pclPointOpt.value();
            Q_EMIT mapPointSelected(nameSpace, pclIndex, pclPoint);

            {
                std::ostringstream sstm;
                sstm << "Point: " << pclPoint.x << ", " << pclPoint.y << ", " << pclPoint.z;
                RVIZ_COMMON_LOG_WARNING(sstm.str().c_str());
            }
        }

        return Render;
    }
    return InteractionTool::processMouseEvent(event);
}


void MapPointTool::activate()
{
    lastCameraPos.x = lastCameraPos.y = lastCameraPos.z = -1.0f;
    lastCameraDir.x = lastCameraDir.y = lastCameraDir.z = -1.0f;

    // Ogre::Camera* camera = context_->getViewManager()->getCurrent()->getCamera();
    // lastCameraPos = camera->getDerivedPosition();
    // lastCameraDir = camera->getDerivedOrientation() * Ogre::Vector3::NEGATIVE_UNIT_Z;

    if ( markerThread ) {
        markerThread->setActive(true);
    }
}

void MapPointTool::deactivate()
{
    lastCameraPos.x = lastCameraPos.y = lastCameraPos.z = -1.0f;
    lastCameraDir.x = lastCameraDir.y = lastCameraDir.z = -1.0f;

    Q_EMIT pointCloudFiltered(pcdPointNormal);

    if(markerThread) {
        markerThread->setActive(false);
    }
}

void MapPointTool::update(float wall_dt, float ros_dt)
{
    Ogre::Camera* camera = context_->getViewManager()->getCurrent()->getCamera();
    Ogre::Vector3 cameraPos = camera->getDerivedPosition();
    Ogre::Vector3 cameraDir = camera->getDerivedOrientation() * Ogre::Vector3::NEGATIVE_UNIT_Z;

    if ((cameraPos - lastCameraPos).squaredLength() > 1e-4 || (cameraDir - lastCameraDir).squaredLength() > 1e-4)
    {
        lastCameraPos = cameraPos;
        lastCameraDir = cameraDir;

        pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdFiltered(new pcl::PointCloud<pcl::PointXYZINormal>);

        int numThreads = omp_get_max_threads();
        std::vector<pcl::PointCloud<pcl::PointXYZINormal>> threadResults;
        threadResults.resize(numThreads);

        // {
        //     std::ostringstream sstm;
        //     sstm << " cameraPos: " << cameraPos.x << ", " << cameraPos.y << ", " << cameraPos.z;
        //     sstm << " threadNum: " << numThreads;
        //     RVIZ_COMMON_LOG_WARNING(sstm.str().c_str());
        // }

#pragma omp parallel
        {
            int tid = omp_get_thread_num();
            auto& partCloud = threadResults[tid];

#pragma omp for
            for (const auto& pt : *pcdPointNormal) {
                Eigen::Vector3f normal(pt.normal_x, pt.normal_y, pt.normal_z);
                Eigen::Vector3f to_camera(cameraPos.x - pt.x, cameraPos.y - pt.y, cameraPos.z - pt.z);
                to_camera.normalize();

                float dot = normal.dot(to_camera);
                if (dot >= 0) {
                    partCloud.push_back(pt);
                }
            }
        }

        for (const auto& threadResult : threadResults) {
            pcdFiltered->insert(pcdFiltered->end(), threadResult.begin(), threadResult.end());
        }

        if(pcdViewPointNormal != pcdPointNormal) {
            pcdViewPointNormal.reset();
        }
        
        pcdViewPointNormal = pcdFiltered;
        Q_EMIT pointCloudFiltered(pcdViewPointNormal);
    }
}

void MapPointTool::load(const rviz_common::Config& config )
{
    // set marker server namespace
    QString name;
    config.mapGetString("Name", &name);
    setName(name);
    context_->getToolManager()->refreshTool(this);
    // set marker server namespace
    QString nameSpace;
    config.mapGetString("NameSpace", &nameSpace);
    markerThread = InteractiveMarkerManager::getInstance()->addThread(nameSpace);
}


void MapPointTool::handlePointCloudLoaded(
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdPointNormal)
{
    this->pcdPointNormal = pcdPointNormal;
    this->pcdViewPointNormal = pcdPointNormal;
}


int MapPointTool::findNearestPointToCamera(
    const Eigen::Vector3f& rayOrigin,
    const Eigen::Vector3f& rayDirection,
    float voxelResolution,
    float radiusSearch
)
{
    // 构建 Octree, 体素分辨率voxelResolution
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZINormal> octree(voxelResolution);
    octree.setInputCloud(pcdViewPointNormal);
    octree.addPointsFromInputCloud();

    pcl::octree::OctreePointCloudSearch<pcl::PointXYZINormal> oriOctree(voxelResolution);
    oriOctree.setInputCloud(pcdPointNormal);
    oriOctree.addPointsFromInputCloud();

    // 获取射线穿过的体素中心
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelCenters(new pcl::PointCloud<pcl::PointXYZINormal>);
    octree.getIntersectedVoxelCenters(rayOrigin, rayDirection, voxelCenters->points);

    if (voxelCenters->points.empty())
    {
        return -1;
    }

    for (const auto& voxelPoint : voxelCenters->points) {

        std::vector<int> indices;
        std::vector<float> dists;

        if (octree.radiusSearch(voxelPoint, radiusSearch, indices, dists) == 0) {
            continue;
        }

        for (auto it = indices.begin(); it != indices.end(); ++it) {
            pcl::Normal normal;
            normal.normal_x = pcdViewPointNormal->points[*it].normal_x;
            normal.normal_y = pcdViewPointNormal->points[*it].normal_y;
            normal.normal_z = pcdViewPointNormal->points[*it].normal_z;
            normal.curvature = pcdViewPointNormal->points[*it].curvature;

            // 获取法线向量
            Eigen::Vector3f normalVec(
                normal.normal_x,
                normal.normal_y,
                normal.normal_z
            );
            normalVec.normalize();

            // 计算夹角
            float angleRad = std::acos(normalVec.dot(rayDirection));
            float angle = angleRad * 180 / M_PI;

            {
                std::ostringstream sstm;
                sstm << "Angle: " << angle << "°";
                RVIZ_COMMON_LOG_WARNING(sstm.str().c_str());
            }

            if (angle > 90) {

                pcl::PointXYZINormal desPoint = pcdViewPointNormal->points[*it];
                // 找到点在原始点云中的index
                std::vector<int> oriIndices;
                std::vector<float> oriDists;

                if (oriOctree.radiusSearch(desPoint, 0.001, oriIndices, oriDists) != 1) {
                    return -1;
                }
                return oriIndices[0];
            }

        }
    }

    return -1;


    // 找到与相机 origin 最近的 voxel center
    // Eigen::Vector3f ei_closest_voxel;
    // float min_voxel_dist = std::numeric_limits<float>::max();
    // for (const auto& center : voxelCenters->points) {
    //     Eigen::Vector3f ei_center(center.x, center.y, center.z);
    //     float dist = (ei_center - rayOrigin).squaredNorm();
    //     if (dist < min_voxel_dist) {
    //         min_voxel_dist = dist;
    //         ei_closest_voxel = ei_center;
    //     }
    // }

    // 对这个最近体素中心调用一次 radiusSearch
    // pcl::PointXYZ closest_voxel(ei_closest_voxel[0], ei_closest_voxel[1], ei_closest_voxel[2]);
    // std::vector<int> indices;
    // std::vector<float> dists;
    // if (octree.radiusSearch(closest_voxel, radiusSearch, indices, dists) == 0)
    //     return std::nullopt;

    // 在这些点中找距离相机 origin 最近的点
    // auto min_it = std::min_element(dists.begin(), dists.end());
    // int min_idx = std::distance(dists.begin(), min_it);
    // 获取对应的点索引
    // int nearest_point_idx = indices[min_idx];
    // pcl::PointXYZ nearest_point = pclPcdPtr->points[nearest_point_idx];
    // return nearest_point;
}




}

}



#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rviz_custom_plugins::tools::MapPointTool, rviz_common::Tool )
